import SimpleOpenNI.*;
SimpleOpenNI kinect;
float handDistance;
float handAngle;

void setup() {
  kinect = new SimpleOpenNI(this);
  kinect.enableDepth();
  kinect.enableUser(SimpleOpenNI.SKEL_PROFILE_ALL);
  size(640, 480);  
  fill(255, 255, 0);
  stroke(0, 0, 255); 
  strokeWeight(3); 
  smooth();  
}
void draw() {
  kinect.update();  
  image(kinect.depthImage(), 0, 0);   
  IntVector userList = new IntVector(); 
  kinect.getUsers(userList);   
  if (userList.size() > 0) {  
    int userId = userList.get(0); 
    if ( kinect.isTrackingSkeleton(userId)) {  
      drawSkeleton(userId);
      println("success draw Skeleton");
      handAngle= this.getAngle(userId);
      println("success calculate angle");
    }       
  handDistance=getJointDistance(userId,SimpleOpenNI.SKEL_RIGHT_HAND, SimpleOpenNI.SKEL_LEFT_HAND);
  text(handDistance,width-50,30); 
  text(handAngle,width-50,80);
  } 
}  
void drawSkeleton(int userId) {  
  stroke(0); 
  strokeWeight(5);  
  kinect.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HAND, SimpleOpenNI.SKEL_RIGHT_HAND); 
  noStroke();   
  fill(255, 255, 0); 
  drawJoint(userId, SimpleOpenNI.SKEL_RIGHT_HAND);
  drawJoint(userId, SimpleOpenNI.SKEL_LEFT_HAND);
} 
float getAngle(int userId){
  PVector rightHand = new PVector();
  PVector leftHand = new PVector();
  kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_LEFT_HAND,leftHand);
  kinect.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_RIGHT_HAND,rightHand);
  PVector mid = PVector.add(leftHand,rightHand);
  text("lefthand:x "+leftHand.x+"lefthand:y "+leftHand.y+"leftHand.z: "+leftHand.z,50,30);
  text("righthand:x "+rightHand.x+"righthand:y "+rightHand.y+"rightHand.z: "+rightHand.z,50,50);
  text("mid:x "+mid.x+"mid:y "+mid.y+"mid.z: "+mid.z,50,80);
  
  PVector horizon = new PVector(rightHand.x,0,0);
  float angleInRadians = PVector.angleBetween(mid,horizon);
  return(degrees(angleInRadians));


}
float getJointDistance(int userId, int jointID1, int jointID2) {  
  PVector joint1 = new PVector();  
  PVector joint2 = new PVector();  
  kinect.getJointPositionSkeleton(userId, jointID1, joint1); 
  kinect.getJointPositionSkeleton(userId, jointID2, joint2);  
  return PVector.dist(joint1, joint2); } 

void drawJoint(int userId, int jointID) { 
  PVector joint = new PVector();  
  float confidence = kinect.getJointPositionSkeleton(userId, jointID, joint); 
  if (confidence < 0.5) {
    return;  
  }
  // convert the detected hand position 
  // to "projective" coordinates 
  // that will match the depth image
  PVector convertedJoint = new PVector(); 
  kinect.convertRealWorldToProjective(joint, convertedJoint);
  ellipse(convertedJoint.x, convertedJoint.y, 5, 5);
} 

// user-tracking callbacks!  
void onNewUser(int userId) {
  println("start pose detection");
  kinect.startPoseDetection("Psi", userId); 
}   

void onEndCalibration(int userId, boolean successful) {  
  if (successful) {     
  println(" User calibrated !!!"); 
  kinect.startTrackingSkeleton(userId);  
  }
  else {
    println(" Failed to calibrate user !!!");
    kinect.startPoseDetection("Psi", userId);  
  } 
} 

void onStartPose(String pose, int userId){
  println("Started pose for user"); 
  kinect.stopPoseDetection(userId); 
  kinect.requestCalibrationSkeleton(userId, true); 
}
